#include "Gyro.h"
#include <iic.h>


#if defined(ARDUINO) && ARDUINO >= 100
#include "Arduino.h"
#else
#include "wiring.h"
#endif

#define ITG3200_ADDRESS 0XD0
#define ITG3200_DLPF_CFG 2

const float GYRO_SCALE = PI / (180.0f * 13.75f);//14.375f);//14.375f);

void Gyro::init()
{
    delay(100);
    i2c_writeReg(ITG3200_ADDRESS, 0x3E, 0x80); //register: Power Management  --  value: reset device
    delay(5);
    i2c_writeReg(ITG3200_ADDRESS, 0x16, 0x18 + ITG3200_DLPF_CFG); //register: DLPF_CFG - low pass filter configuration
    delay(5);
    i2c_writeReg(ITG3200_ADDRESS, 0x3E, 0x03); //register: Power Management  --  value: PLL with Z Gyro reference
    delay(100);
    calibrating = 0;
    vector.zero();
}

void Gyro::calibrate()
{
    calibrating = 128;
    for (uint8_t axis = 0; axis < 3; axis++)
    {
        sum[axis] = 0;
        zero[axis] = 0;
    }
    vector.zero();
}

void Gyro::read(int16_t out[])
{
    i2c_getSixRawADC(ITG3200_ADDRESS, 0X1D);
    out[0] = ((rawADC[2]<<8) | rawADC[3]);
    out[1] = -((rawADC[4]<<8) | rawADC[5]);
    out[2] = -((rawADC[0]<<8) | rawADC[1]);
}

void Gyro::update()
{
    //TWBR = ((16000000L / 400000L) - 16) / 2; // change the I2C clock rate to 400kHz

    read(raw);
    

   // static int16_t previousdata[3] = {0,0,0};
    uint8_t axis;
    
    if (calibrating>0)
    {
        for (axis = 0; axis < 3; axis++)
        {
            // Sum up 400 readings
            sum[axis] += raw[axis];
            // Clear global variables for next reading
            raw[axis]=0;
            if (calibrating == 1)
                zero[axis] = sum[axis] >> 7;
        }
        calibrating--;

    }
    
    /*
    for (axis = 0; axis < 3; axis++)
    {
        //raw[axis]  -= zero[axis];
        //anti gyro glitch, limit the variation between two consecutive readings
        //   raw[axis] = constrain(raw[axis], previousdata[axis]-800, previousdata[axis]+800);
        //   previousdata[axis] = raw[axis];

        vector[axis] = (raw[axis] - zero[axis]) * GYRO_SCALE;
    }*/
    
    vector.x = (raw[0] - zero[0]) * GYRO_SCALE;
    vector.y = (raw[1] - zero[1]) * GYRO_SCALE;
    vector.z = (raw[2] - zero[2]) * GYRO_SCALE;
    
}
